#ifndef __LOOPCLOSING_H__
#define __LOOPCLOSING_H__
#include "../include/Typedef.hpp"
#include "../include/Frame.hpp"
#include "../include/MapPoint.hpp"
#include "../include/Matcher.hpp"
#include "../include/Param.hpp"
#include "../include/Optimizer.hpp"
#include "../include/Debuger.hpp"
#include "../include/KeyFrameDatabase.hpp"
#include "../include/Map.hpp"
#include "../include/LocalMap.hpp"
#include "../include/MapAtlas.hpp"
#include <mutex>
#include "../include/Sim3Solver.hpp"
#include <thread>
using namespace std;
namespace ORBSLAM
{
    class LocalMap;
    class Track;
    class Map;
    class KeyFrameDatabase;
    // class Optimizer;
    class LoopClosing
    {
    public:
        typedef pair<set<KeyFrame *>, int> type_consistent_group;
        typedef map<KeyFrame *, g2o::Sim3, less<KeyFrame *>, Eigen::aligned_allocator<pair<KeyFrame *const, g2o::Sim3>>> type_keyframe_and_pose;

    private:
        /* data */
        bool mb_finished;
        // list<KeyFrame *> mlp_new_keyframe;
        KeyFrame *mp_current_keyframe;
        MapAtlas *mp_mapatlas;
        Map *mp_last_map;
        Map *mp_map_to_reset;
        Matcher *mp_matcher;
        Param *mp_param;

        KeyFrame *mp_loop_matched_keyframe;
        KeyFrame *mp_loop_last_cur_keyframe;
        KeyFrame *mp_merge_matched_keyframe;
        KeyFrame *mp_merge_last_cur_keyframe;

        g2o::Sim3 msim3_loop_Scw;
        g2o::Sim3 msim3_merge_Scw;

        g2o::Sim3 msim3_loop_Slw;
        g2o::Sim3 msim3_merge_Slw;

        g2o::Sim3 msim3_old_new;

        vector<MapPoint *> mvp_loop_window_mpt;
        vector<MapPoint *> mvp_loop_matched_mpt;
        vector<MapPoint *> mvp_merge_window_mpt;
        vector<MapPoint *> mvp_merge_matched_mpt;

        vector<MapPoint *> mvp_loop_mpt_for_fusion;

        list<KeyFrame *> mlp_loop_keyframe_queue;

        int mn_loop_coincidence_num;
        int mn_merge_coincidence_num;
        bool mb_loop_detected;
        bool mb_merge_detected;
        bool mb_fix_scale;

        int mn_loop_not_found_num;
        int mn_merge_not_found_num;

        KeyFrameDatabase *mp_keyframe_database;

        LocalMap *mp_localmap;

        bool mb_runing_gba;
        bool mb_stop_gba;
        bool mb_reset_requested;
        bool mb_reset_activemap_requested;
        bool mb_finish_requested;

        int mn_full_ba_num;

        unsigned long mn_last_loop_keyframe_id;

        thread *mp_thread_gba;

        vector<KeyFrame *> mvp_current_connected_keyframe;

    public:
        LoopClosing(/* args */);
        ~LoopClosing();
        LoopClosing(KeyFrameDatabase* p_keyframe_database);
        void Set_param(Param *p_param);
        void Set_localmap(LocalMap* p_localmap);
        void Run();
        void Insert_keyframe(KeyFrame* p_keyframe);
        bool Check_new_keyframe_exist();
        bool Detect_common_region();
        bool Detect_common_regions_from_bow(vector<KeyFrame *> &vp_bow_cand, KeyFrame *p_matched_keyframe2, KeyFrame *p_last_current_keyframe, g2o::Sim3 &g2o_Scw, int &n_coincidence_num, vector<MapPoint *> &vp_mpt, vector<MapPoint *> &vp_matched_mpt);
        bool Check_common_region_from_cov_keyframe(KeyFrame *p_keyframe_j, KeyFrame *p_candidate_keyframe, g2o::Sim3 &g_Scw, int &n_proj_match_num, vector<MapPoint *> &vp_candidate_mpt, vector<MapPoint *> &vp_matched_mpt);
        int Find_match_by_projection(KeyFrame *p_keyframe_j, KeyFrame *p_candidate_keyframe, g2o::Sim3 &g_Scw, vector<MapPoint *> &vp_window_mpt, vector<MapPoint *> &vp_matched_mpt);
        bool Detect_and_refine_sim3_from_last_keyframe(KeyFrame *p_cur_keyframe, KeyFrame *p_matched_keyframe, g2o::Sim3 &sim3_Scw, int &n_proj_matched_num, vector<MapPoint *> &vp_window_mpt, vector<MapPoint *> &vp_matched_mpt);
        void Correct_loop();
        void Search_and_fuse(const type_keyframe_and_pose &corrected_map, vector<MapPoint *> &vp_mpt_for_fusion);

        bool Is_runing_gba();

        void Reset_if_requested();
        bool Check_finish();
        void Set_finish();

    public:
        mutex mmutex_loop_queue;
        mutex mmutex_reset;
        mutex mmutex_finish;
        mutex mmutex_gba;
    };

}
#endif // __LOOPCLOSING_H__